﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Linq;
using System.Windows.Forms;
using System.Net.Sockets;
using System.Drawing.Imaging;
using System.Threading;
using System.Diagnostics;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Util;

namespace NAO_Image_Processing
{
    public partial class GUI : Form
    {
        /*DEBUG*/
        int counter = 1;
        string prefix = "C:\\Dokumente und Einstellungen\\Michael\\Desktop\\";

        /* Statischer Block */
        private static int KCOLORSPACE = 13;

        /* Klassendeklaration */
        private NAOControl _naoControl;
        private NAOCamera _camera;
        private ImageProcessor _imageProcessor;

        /* Variablendeklaration */
        private int kFps;
        private int resolutionHeigth;
        private bool _naoCamInitialised;
        private bool updatingPicture;
        private double abstandFussBoden, hoeheKamera, abstandKameraBoden;

        private NaoCamImageFormat _naoCamCurrentImageFormat;
        private Bitmap bitmap;
        private Image<Bgr, Byte> imgOrig;
        private Image<Bgr, Byte> imgEdit;
        private List<String> listResolution;
        private List<int> listFPS;
        private System.Threading.Timer timer;
        private System.Threading.TimerCallback timerDelegate;

        public GUI()
        {
            InitializeComponent();
            _camera = new NAOCamera();
            _imageProcessor = new ImageProcessor();
            _naoCamCurrentImageFormat = _camera.NaoCamImageFormats[1];
            timerDelegate = new TimerCallback(DrawNaoCamImageToPictureBox);
        }

        private void beendenToolStripMenuItem_Click(object sender, EventArgs e)
        {
            if (_camera != null)
            {
                _camera.Disconnect();
            }
            System.Windows.Forms.Application.Exit();
        }

        private void btn_check_Click(object sender, EventArgs e)
        {
            /*Überprüfen ob der NAO unter der angegebenen IP-Adresse verfügbar ist.
             *Es wird überprüft, ob eine Verbindung mit dem NAOqi des NAO hergestellt werden kann.
             */
            try
            {
                TcpClient tcpclient = new TcpClient(tB_naoIP.Text, Convert.ToInt32(tB_naoPort.Text));
                lblNaoStatus.Text = "NAO gefunden";
                lblNaoStatus.ForeColor = Color.Green;
                tcpclient.Close();
            }
            catch (SocketException ex)
            {
                lblNaoStatus.Text = "Kein NAO gefunden";
                lblNaoStatus.ForeColor = Color.Red;
                Console.Error.WriteLine("Error connecting to NAO: " + ex.Message);
                return;
            }
            /* Kontroll- und Kameraobjekte erzeugen */
            _naoControl = new NAOControl(tB_naoIP.Text, Convert.ToInt32(tB_naoPort.Text));
            _camera = new NAOCamera();
            
            /* GroupBoxes freigeben */
            if (_naoControl.isMotionAvailable())
            {
                gBMotionControl.Enabled = true;
            }
            gBCameraControl.Enabled = true;
            /* Batteriestand auslesen und anzeigen */
            if (_naoControl.isBatteryAvailable() && !_naoControl.DEBUG)
            {
                lbl_BatteryPercent.Text = "" + _naoControl._batteryProxy.getBatteryCharge() + " %";
            }
            
            /* Mögliche Posen in die ComboBox schreiben */
            if (_naoControl.isRobotPostureAvailable())
            {
                cmbPoses.DataSource = _naoControl._robotPostureProxy.getPostureList();
            }

            /* Stiffness auslesen und anzeigen */
            checkStiffness();
            /* Auflösungen in die ComboBox schreiben */
            listResolution = new List<string>();
            for (int i = 0; i < _camera.NaoCamImageFormats.Count; i++)
            {
                listResolution.Add(_camera.NaoCamImageFormats.ElementAt(i).name);
            }
            cmbResolution.DataSource = listResolution;
            resolutionHeigth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[1]);
            /* FPS in die ComboBox schreiben */
            listFPS = new List<int>();
            listFPS.Add(1); listFPS.Add(2); listFPS.Add(3); listFPS.Add(5);
            cmbFPS.DataSource = listFPS;
        }

        private void btn_Say_Click(object sender, EventArgs e)
        {
            if (_naoControl.isConnected())
            {
                //_naoControl._ttsProxy.setLanguage("English");
                //_naoControl._ttsProxy.say(tb_Say.Text);
            }
        }

        private void btn_ExecMotion_Click(object sender, EventArgs e)
        {
            List<string> liste = _naoControl._robotPostureProxy.getPostureList();
            _naoControl._robotPostureProxy.goToPosture(Convert.ToString(cmbPoses.SelectedItem), 0.75f);
        }

        private void btn_connectCamera_Click(object sender, EventArgs e)
        {
            resolutionHeigth = Convert.ToInt32(((String)(cmbResolution.SelectedItem)).Split('*')[1]);
            _imageProcessor.setScreenParameters(resolutionHeigth);
            kFps = Convert.ToInt32(cmbFPS.Text);
            for (int i = 0; i < _camera.NaoCamImageFormats.Count; i++)
            {
                if (_camera.NaoCamImageFormats.ElementAt(i).name == cmbResolution.Text)
                {
                    _naoCamCurrentImageFormat = _camera.NaoCamImageFormats.ElementAt(i);
                }
            }
            try
            {
                if (_camera.Connect(_naoControl.getIP(), _naoCamCurrentImageFormat, KCOLORSPACE, kFps))
                {
                    lblCameraStatus.Text = "Verbunden";
                    lblCameraStatus.ForeColor = Color.Green;
                }
                timer = new System.Threading.Timer(timerDelegate);
                _naoCamInitialised = true;
            }
            catch (Exception ex)
            {
                _naoCamInitialised = false;
                Console.Out.WriteLine("Exception while connecting to NaoCam: " + ex);
            }
            if (_naoCamInitialised)
            {

                if (_naoControl.DEBUG)
                {
                    timer.Change(0, (int)Math.Ceiling(10000.0 / kFps));
                }
                else
                {
                    timer.Change(0, (int)Math.Ceiling(1000.0 / kFps));
                }
                timerGUI.Start();
            }
            if (_camera.visionIsAvailable())
            {
                gBKinematik.Enabled = true;
            }
        }

        void UpdateScreen(object sender, EventArgs e)
        {
            if (!updatingPicture)
            {
                DrawMotionInformation();
                lblHeadPitchStatus.Text = "HeadPitch: " + Math.Round(_naoControl.getCamAngle(), 2);
                if (_naoControl.isBatteryAvailable() && !_naoControl.DEBUG)
                {
                    lbl_BatteryPercent.Text = _naoControl._batteryProxy.getBatteryCharge() + " %";
                }
                
            }
        }

        /*           /| b_1 
         *          / |  
         *         /al|
         *        /   | 
         *       /    |
         *      /-----| b_2
         *     /|\    |
         * c  /a| \   | 
         *   /  |  \ga| 
         *  /   |   \ |
         * /be  | ga2\|
         * -----------------
         * a_c  a_x
         * b=motionProxy.getPosition("CameraTop", 1, true)[2]*100 --> Höhe des Kopfes über dem Boden in cm
         * a=b*tan(alpha)                               Z
         * c=sqrt(a^2+b^2)                              |   
         * ################                             |    
         * Kameraversatz in Y-Richtung = 1.2°          / \
         * Horizontaler Blickwinkel = 47.6°           /   \
         * Vertikaler Blickwinkel = 60.9°            /     \
         * Head-Yaw [Z] = 239° [-119.5° - 119.5°]   X       Y
         * Head-Pitch [X] = 68° [-38.5° - 29.5°]
         */
        private void DrawMotionInformation()
        {
            List<double> kinematic = _naoControl.calcKinematics();
            hoeheKamera = kinematic[1];
            abstandFussBoden = kinematic[0];
            abstandKameraBoden = kinematic[2];
            lblHoeheKamera.Text = hoeheKamera + " cm";
            lblAbstandFussBoden.Text = abstandFussBoden + " cm";
            lblAbstandKameraBoden.Text = abstandKameraBoden + " cm";
        }

        void DrawNaoCamImageToPictureBox(object stateInfo)
        {
            if (_naoCamInitialised && !updatingPicture)
            {
                updatingPicture = true;
                /* Bild von der Kamera holen */
                bitmap = new Bitmap(_camera.GetImage(_naoCamCurrentImageFormat));
                imgOrig = new Image<Bgr, Byte>(bitmap);
                imgEdit = new Image<Bgr, Byte>(bitmap);

                /* Ausgabebild bearbeiten */
                if (cbDistance.Checked)
                {
                    imgEdit = _imageProcessor.drawDistance(imgEdit, _naoControl.getDistances(), resolutionHeigth);
                }
                if (cbSURF.Checked)
                {
                    imgEdit = _imageProcessor.SURF(imgOrig, imgEdit);
                }
                if (cbHough.Checked)
                {
                    imgEdit = _imageProcessor.HoughLines(imgOrig, imgEdit);
                }
                /* Bilder zeichnen */
                imgBoxOrig.Image = imgOrig;
                imgBoxEdit.Image = imgEdit;
                /*Bildercache leeren*/
                bitmap = null;
            }
            updatingPicture = false;
        }

        private void btnDisconnectCamera_Click(object sender, EventArgs e)
        {
            try
            {
                timer.Change(System.Threading.Timeout.Infinite, System.Threading.Timeout.Infinite);
            }
            catch (ObjectDisposedException ex)
            {
                System.Console.WriteLine("Error while disposing timer: " + ex.Message);
            }
            timer.Dispose();
            timerGUI.Stop();
            if (!timerGUI.Enabled && _camera.Disconnect())
            {
                imgBoxOrig.Image = new Image<Bgr, Byte>(NAO_Image_Processing.Properties.Resources.no_pic_1);
                imgBoxEdit.Image = new Image<Bgr, Byte>(NAO_Image_Processing.Properties.Resources.no_pic_2);
                lblCameraStatus.Text = "nicht Verbunden";
                lblCameraStatus.ForeColor = Color.Red;
            }
        }

        private void btn_ToggleStiffness_Click(object sender, EventArgs e)
        {
            toggleStiffness();
        }

        private void toggleStiffness()
        {
            if (checkStiffness())
            {
                _naoControl._motionProxy.setStiffnesses("Body", 1);
                
            }
            else
            {
                _naoControl._motionProxy.setStiffnesses("Body", 0);
            }
        }

        private bool checkStiffness()
        {
            List<float> stiffnesses = _naoControl._motionProxy.getStiffnesses("Body");
            if (stiffnesses[0] == 1.0f)
            {
                lblStiffnessStatus.Text = "Enabled";
                return true;
            }
            else
            {
                lblStiffnessStatus.Text = "Disabled";
                return false;
            }
        }

        private void textBox1_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == '\r' && tBHeadPitch.Text != "")
            {
                try
                {
                    object name = "HeadPitch";
                    object change = (float)(Convert.ToDouble(tBHeadPitch.Text) * Math.PI / 180);
                    float speed = 0.1f;
                    _naoControl._motionProxy.setStiffnesses("Head", 1.0f);
                    _naoControl._motionProxy.setAngles(name, change, speed);
                }
                catch (FormatException ex)
                {
                    Console.Out.WriteLine("Exception while parsing HeadPitch: " + ex.Message);
                }
            }
        }

        private void imgBoxOrig_Click(object sender, EventArgs e)
        {
            if (_naoControl.DEBUG_PICTURE_MAKER)
            {
                //imgBoxOrig.Image.Save(prefix + "Bild_" + counter + ".jpg");
                imgBoxEdit.Image.Save(prefix + "Bild_" + counter + ".jpg");
                counter++;
            }
        }
    }
}